/**
 * File: robot_control_util.cpp
 * Brief: 
 * Created by: zhangping
 * Created at: 2017.07.17
 * Modified by:
 * Modified at:
 */

#include "ros/ros.h"
#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include "robot_control_util.h"
#include "kinematic.h"


namespace robot_node {

/**
 * This callback function will forward Institution message to our Robot.
 */
void ControlThread::_PubIns(Communication &com, const geometry_msgs::Twist::ConstPtr& msg)
{
    ROS_INFO("Twist> [T=%f,W=%f]", msg->linear.x, msg->angular.z);
    U8 x[2] = {0}; // move at the x-axis of car, move right
    U8 y[2] = {0}; // move at the y-axis of car, move foward
    U8 z[2] = {0}; // rotation, 
    U8 orientation = 0;
    
    /* 注意：小车的x轴对应于Twist的y轴，小车的y轴对应于Twist的x轴。小车的y轴对应于前进速度。*/
    kinematic::U16 move_foward = kinematic::LinearVelocityToPulse(msg->linear.x);
    kinematic::U16 move_right = kinematic::LinearVelocityToPulse(msg->linear.y);
    kinematic::U16 rotation = kinematic::RotationVelocityToPluse(msg->angular.z);
    kinematic::U8  direction = kinematic::ComputeDirection(msg->linear.y
                                        , msg->linear.x, msg->angular.z);

    y[0] = move_foward/255;
    y[1] = move_foward%255;
    x[0] = move_right/255;
    x[1] = move_right%255;
    z[0] = rotation/255;
    z[1] = rotation%255;
    orientation = direction;
    
    ROS_INFO("Car> [ %x %x %x %x %x %x %x]", x[0], x[1]
                                           , y[0], y[1]
                                           , z[0], z[1]
                                           ,orientation);
    if( !(com.SendCmdWithArgs( kPosMode, x, y ,z, orientation)) ){
        std::cerr<<"Error: Send Insititution failed"<<std::endl;
    }
}

typedef const boost::function<void (const geometry_msgs::Twist::ConstPtr& msg)> InsCallback;

void* ControlThread::_thread_handler(void *this_)
{
    ControlThread *obj = (ControlThread *)this_;
    InsCallback ins_callback = boost::bind( obj->_PubIns, boost::ref(obj->com_), _1);
    ros::Subscriber sub = obj->node_.subscribe("/cmd_vel", 100, ins_callback);
    ros::spin();
    return 0;
}


void ControlThread::_clean_threads(){
    pthread_cancel(this->tid_);
}


ControlThread::ControlThread(ros::NodeHandle &n, Communication &com)
:node_(n),com_(com)
{
    pthread_t tid=-1;
    if(0!=pthread_create(&tid, NULL, this->_thread_handler, this)){
        std::cout<<"error: falied at @pthread_create."<<std::endl;
            this->_clean_threads();
    }
    this->tid_ = tid;
}


ControlThread::~ControlThread()
{
    this->_clean_threads();
}


pthread_t ControlThread::GetPid()
{
    return this->tid_;
}

} // End of namespace
